function [xOut, mse] = selfLocNbp1(initalisation, parameters, topology, measurements)

%--------------------get parameters and data from structures----------------------------
N = parameters.N;
T = parameters.MPiter;
A = parameters.A;
W = parameters.W;
var_v = parameters.varV;
var_m = parameters.varM;
N_particles = parameters.sampling.numParticles;

meanInit = initalisation.mean;
covInit = initalisation.cov;

con_ndac = topology.conNdAc;
con_ndnd = topology.conNdNd;
trajec = topology.trajec;
p_ac = topology.ac;
n_ac = length(p_ac);
ndist_ndac = measurements.NdAc;
ndist_ndnd = measurements.NdNd;


%--------------------initialize vectors-------------------------------------------------
[len_state, ~, n_nd] = size(trajec);
xOut = zeros(4,n_nd,N+1);
bnd_est = zeros(N_particles,len_state,n_nd);
bnd_pred = zeros(N_particles,len_state,n_nd);

for i=1:n_nd
    bnd_est(:,:,i) = mvnrnd(meanInit(:,i),covInit(:,:,i),N_particles);
    xOut(:,i,1) = meanInit(:,i);
end


%--------------------iterate over all time steps----------------------------------------
for n = 1:N
    bnd = zeros(N_particles,len_state,n_nd,2);
    con_nd = sum(con_ndnd,2);
    con_ac = sum(con_ndac,2);
    
    %--------------------perform prediction----------------------------------------------
    for i = 1:n_nd
        bnd_pred(:,:,i) = (A*bnd_est(:,:,i)' + W*sqrt(var_v)*[randn(1,N_particles); randn(1,N_particles)])';
    end
    
    %--------------------perform message passing iterations------------------------------
    for t=1:T
        idx1 = mod(t,2) + 1;
        idx2 = -mod(t,2) + 2;
        
        if (t == 1)
            bnd(:,:,:,idx1) = bnd_pred;
        end
        
        %--------------------iterate over all mobile agents------------------------------
        for i = 1:n_nd
            p_acs = zeros(con_ac(i),2);
            ndists_ndac = zeros(con_ac(i),1);
            messages = cell(con_nd(i),1);
            tmp = ones(N_particles,2);
            
            proposal = bnd_pred(:,:,i);            
            
            %--------------------calculate messages from static anchors------------------
            k = 1;
            for j = 1:n_ac
                if (con_ndac(i,j))
                    ndists_ndac(k) = ndist_ndac(i,j,n);
                    p_acs(k,:) =  p_ac(j,:);
                    k = k+1;
                end
            end
            
            %--------------------calculate messages from mobile agents-------------------
            k = 1;
            for j = 1:n_nd
                if (con_ndnd(i,j) && i ~= j)
                    angle = 2 * pi *rand(N_particles,1);
                    v = sqrt(var_m)*randn(N_particles,1);
                    
                    tmp(:,1) = bnd(:,1,j,idx1) + (ndist_ndnd(i,j,n)+v).*cos(angle);
                    tmp(:,2) = bnd(:,2,j,idx1) + (ndist_ndnd(i,j,n)+v).*sin(angle);
                    messages{k} = tmp;
                    k = k + 1;
                end
            end
            
            %--------------------perform message multiplication-------------------------
            bnd(:,:,i,idx2) = messageMultiplication1(messages, proposal, p_acs, ndists_ndac, var_m);
            
            for l=1:len_state
                xOut(l,i,n+1) = mean(bnd(:,l,i,idx2));
            end
        end
    end
    for i=1:n_nd
        bnd_est(:,:,i)  = bnd(:,:,i,idx2);
    end
end

%--------------------calculate MSE------------------------------------------------------
mse = zeros(N+1,n_nd);
for m=1:n_nd
    tmp = xOut(:,m,:);
    for i=1:N+1
        mse(i,m) = norm(trajec(:,i,m)-tmp(:,i))^2;
    end
end

end